/******************************************************************************
 *
 * MotionViewer
 * Copyright (C) 2007 Sangyoon Lee, Electronic Visualization Laboratory, 
 * University of Illinois at Chicago
 *
 * This software is free software; you can redistribute it and/or modify it 
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either Version 2.1 of the License, or 
 * (at your option) any later version.
 *
 * This software is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 
 * License for more details.
 * 
 * You should have received a copy of the GNU Lesser Public License along
 * with this software; if not, write to the Free Software Foundation, Inc., 
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 * Questions or comments about MotionViewer should be directed to 
 * cavern@evl.uic.edu
 *
 *****************************************************************************/

#pragma warning(disable:4996)	
#include <ogreplatform.h>
#if OGRE_PLATFORM == OGRE_PLATFORM_APPLE
#include <Carbon/Carbon.h>
#endif


#include "ViconSubject.h"
#include "LLXStr.h"


ViconSubject::ViconSubject(const char* name): m_sName(name)
{
}

ViconSubject::~ViconSubject()
{
}
	
void ViconSubject::loadSubjectFile(const char* filename)
{

	DOMElement* element, *cElement;
	DOMNodeList* node_list;

	// init xml parser
	LLXMLParser *parser;
	parser = new LLXMLParser();
	parser->initialize();

	// load xml file
	parser->createDocumentFromFile(filename);
	element = parser->getRootElement();				// KinematicModel
	
	// read skeleton
	node_list = parser->getElements("Skeleton");
	int nodes = node_list->getLength();
	
	// skeleton should be unique and only one
	if (nodes != 1)
		return;
	
	element = (DOMElement*)node_list->item(0);
	cElement = (DOMElement*)parser->getChildren(element, "Segment")->item(0);

	// start recursive bone construction
	Matrix3 mat = Matrix3::IDENTITY;

	// start bone parsing
	addBone(parser, cElement, mat);

	delete parser;

	m_sFullPathName = std::string(filename);

#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32	
	int pos = m_sFullPathName.find_last_of("\\");
#else
	int pos = m_sFullPathName.find_last_of("/");
#endif

	m_sFileName = m_sFullPathName.substr(pos+1);
}

void ViconSubject::addBone(LLXMLParser *parser, DOMElement* element, Matrix3& parentMat, int parent)
{
	// read attribute of NAME, ORIENTATION
	char* name = parser->getCharAttribute(element, "NAME");
	char* ori = parser->getCharAttribute(element, "ORIENTATION");
	
	// tokenizer orientation value
	double rot[3];
	if (strlen(ori) != 0)
	{
		char seps[] = " ";
		char* str = strtok(ori, seps);	rot[0] = -atof(str);
		str = strtok(NULL, seps);		rot[2] = atof(str);
		str = strtok(NULL, seps);		rot[1] = atof(str);
	}
	else
		rot[0] = rot[1] = rot[2] = 0.0f;

	// add bone to vector
	VBone bone;
	bone.Name = std::string(name);

	bone.Rotation[0] = rot[0];
	bone.Rotation[1] = rot[1];
	bone.Rotation[2] = rot[2];
	
	bone.ParentMatrix = parentMat;
	bone.ParentInvMatrix = parentMat.Inverse();
	
	bone.InitialMatrix.FromEulerAnglesXYZ(Radian(rot[0]), Radian(rot[1]), Radian(rot[2]));
	
	bone.iParent = parent;
	m_vBones.push_back(bone);
	
	parent = m_vBones.size() -1;
	delete name;
	delete ori;

	// have to traverse all direct children in recursive fashion
	DOMElement* child;
	DOMNode* siblings = parser->getChildren(element, "Segment")->item(0);
	while(siblings != NULL)
	{
		if (siblings && siblings->getNodeType() == DOMNode::ELEMENT_NODE)
		{
			child = (DOMElement*)siblings;
			Matrix3 mat = parentMat * bone.InitialMatrix;
			addBone(parser, child, mat, parent);
		}
		siblings = siblings->getNextSibling();
	}
}

void ViconSubject::computeQuaternion(int id, float* qt)
{
	
	Matrix3 currMat, localMat;
	
	Quaternion q(qt[0], qt[1], qt[2], qt[3]);
	q.ToRotationMatrix(currMat);
	
	// local matrix based on zero binding pose (not vicon binding pose)
	localMat = currMat * m_vBones[id].InitialMatrix.Inverse();
	
	// correct rotation axis based on inherited from parent at binding pose
	q.FromRotationMatrix(localMat);
	Vector3 v = m_vBones[id].ParentMatrix * Vector3(q[1], q[2], q[3]);
	q[1] = v[0];
	q[2] = v[1];
	q[3] = v[2];
	
	qt[0] = q[0];
	qt[1] = q[1];
	qt[2] = q[2];
	qt[3] = q[3];
	
}

void ViconSubject::computeRotation(int id, float* rot)
{
	// for debug
	float x = Radian(rot[0]).valueDegrees();
	float y = Radian(rot[1]).valueDegrees();
	float z = Radian(rot[2]).valueDegrees();

	Matrix3 M, mat;
	mat.FromEulerAnglesXYZ(Radian(rot[0]), Radian(rot[1]), Radian(rot[2]));
	mat = mat * m_vBones[id].InitialMatrix.Inverse();

	
	Radian rx, ry, rz;
	mat.ToEulerAnglesXYZ(rx, ry, rz);
	
	rot[0] = rx.valueRadians();
	rot[2] = ry.valueRadians();
	rot[1] = -rz.valueRadians();

	x = rx.valueDegrees();
	y = ry.valueDegrees();
	z = rz.valueDegrees();
}

void ViconSubject::computeMatrix(int id, Matrix3 &mat)
{

}

bool ViconSubject::writeBVHHierachy(std::ifstream &v)
{
	//HIERARCHY
	//ROOT Reference
	//{
	//	OFFSET X Y Z
	//	CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
	//	JOINT Hips
	//	{
	
	return true;
}

int ViconSubject::getParentBoneID(int childId)
{
	return m_vBones[childId].iParent;
}